package Rangel.policeforce.extaction.clear;

import adf.agent.info.AgentInfo;
import adf.agent.info.ScenarioInfo;
import adf.agent.info.WorldInfo;
import javolution.util.FastSet;
import Rangel.common.world.tools.Util;
import Rangel.common.world.helper.RangelWorldHelper;
import Rangel.common.world.entity.RangelBlockade;
import Rangel.common.world.entity.RangelRoad;
import rescuecore2.misc.Pair;
import rescuecore2.misc.geometry.Line2D;
import rescuecore2.misc.geometry.Point2D;
import rescuecore2.misc.geometry.Vector2D;
import rescuecore2.standard.entities.*;
import rescuecore2.worldmodel.EntityID;

import java.awt.*;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.List;
import java.util.Set;

public class ClearTools {
    private WorldInfo worldInfo;
    private AgentInfo agentInfo;
    private ScenarioInfo scenarioInfo;
    int maxClearDistance;
    private RangelWorldHelper worldHelper;

    public ClearTools(RangelWorldHelper worldHelper, WorldInfo worldInfo, AgentInfo agentInfo, ScenarioInfo scenarioInfo) {
        this.worldInfo = worldInfo;
        this.agentInfo = agentInfo;
        this.scenarioInfo = scenarioInfo;
        this.worldHelper = worldHelper;
        maxClearDistance = scenarioInfo.getClearRepairDistance();
    }

    public List<GuideLine> getPathGuidelines(List<EntityID> path, Point2D destinationPoint) {
        List<GuideLine> guideLineList = new ArrayList<>();

        if (path == null) {
            return null;
        }
        Area sourceArea;
        if (path.isEmpty()) {
            sourceArea = (Area) worldInfo.getEntity(agentInfo.getPosition());
        } else {
            sourceArea = (Area) worldInfo.getEntity(path.get(0));
        }
        Point2D firstPoint = Util.getPoint(worldInfo.getLocation(sourceArea));
        if (path.size() > 1) {
            Edge edgeTo = agentInfo.getPositionArea().getEdgeTo(path.get(1));
            if (edgeTo != null) {
                Point2D mid1 = Util.getMiddle(edgeTo.getLine());
                Point2D agentPosition = Util.getPoint(worldInfo.getLocation(agentInfo.getID()));//current agent location
                Line2D agentEdgeLine = new Line2D(agentPosition, mid1);
                Line2D agentAreaLine = new Line2D(agentPosition, firstPoint);
                Line2D areaEdgeLine = new Line2D(firstPoint, mid1);
                double theta = Util.angleBetween2Lines(agentAreaLine, areaEdgeLine);
                double alpha = Util.angleBetween2Lines(agentAreaLine, agentEdgeLine);
                if (alpha < 80 && theta > 80) {
                    firstPoint = agentPosition;
                }
            }
        }
        Area area;
        Edge edge;
        GuideLine guideLine;
        for (int i = 0; i < path.size() - 1; i++) {
            EntityID id = path.get(i);
            area = (Area) worldInfo.getEntity(id);
            edge = area.getEdgeTo(path.get(i + 1));
            if (edge == null) {
                break;
            }
            Point2D middle = Util.getMiddle(edge.getLine());
            guideLine = new GuideLine(firstPoint, middle);
            guideLineList.add(guideLine);
            firstPoint = middle;
        }
        Point2D lastPoint;
        if (destinationPoint == null) {
            Area lastArea = (Area) worldInfo.getEntity(path.get(path.size() - 1));
            lastPoint = Util.getPoint(worldInfo.getLocation(lastArea));
        } else {
            lastPoint = destinationPoint;
        }

        if (firstPoint.equals(lastPoint)) {
        } else {
            guideLine = new GuideLine(firstPoint, lastPoint);
            guideLineList.add(guideLine);
        }

        return guideLineList;
    }

    public GuideLine getTargetGuideLine(List<GuideLine> guideLines, List<EntityID> path, double clearDistance) {
        if (guideLines.isEmpty()) {
            return null;
        }
        double secureAngle = 30d;
        GuideLine guideLine = guideLines.get(0);

        GuideLine targetGuideLine;
        targetGuideLine = guideLine;
        List<EntityID> areas = new ArrayList<>();
        areas.add(path.get(0));
        double angle;
        List<GuideLine> tempGuidelines = new ArrayList<>();
        boolean firsLine = true;
        for (int i = 1; i < guideLines.size(); i++) {
            guideLine = guideLines.get(i);
            angle = Util.angleBetween2Lines(targetGuideLine, guideLine);
            double distance = (targetGuideLine.getLength() + guideLine.getLength());
            double distanceCost = 1 - (guideLine.getLength() / distance);
            if ((firsLine && distance < clearDistance / 5) || angle < (secureAngle * distanceCost)) {
                Vector2D newDirection = targetGuideLine.getDirection().add(guideLine.getDirection());
                targetGuideLine = new GuideLine(targetGuideLine.getOrigin(), newDirection);
                tempGuidelines.add(targetGuideLine);

                if (path.size() > i) {
                    areas.add(path.get(i));
                }
                targetGuideLine.getAreas().addAll(areas);
            } else {
                break;
            }
            firsLine = false;
        }
        targetGuideLine.setAreas(areas);

        for (GuideLine line1 : tempGuidelines) {
            double a = Util.angleBetween2Lines(line1, targetGuideLine);
            double dist = line1.getLength() * Math.sin(Math.toRadians(a));
            if (dist > scenarioInfo.getClearRepairRad()) {
                targetGuideLine = line1;
                break;
            }
        }
        return targetGuideLine;
    }

    public boolean isOnBlockade(Point2D point2D, List<Area> areas) {
        Blockade blockade;
        Polygon polygon;
        for (Area area : areas) {
            if (!area.isBlockadesDefined()) {
                continue;
            }
            for (EntityID blockadeID : area.getBlockades()) {
                blockade = (Blockade) worldInfo.getEntity(blockadeID);
                polygon = Util.getPolygon(blockade.getApexes());
                if (polygon.contains(point2D.getX(), point2D.getY())) {
                    return true;
                }

            }
        }
        return false;
    }


    public boolean anyBlockadeIntersection(Area area, Line2D targetLine) {
        if (!(area instanceof Road) || targetLine == null) {
            return false;
        }
        RangelRoad RangelRoad = getRangelRoad(area.getID());
        for (RangelBlockade RangelBlockade : RangelRoad.getRangelBlockades()) {
            if (Util.hasIntersection(RangelBlockade.getPolygon(), targetLine)) {
                return true;
            }
        }
        return false;
    }


    public boolean isInClearRange(Blockade target) {
        Pair<Integer, Integer> location = worldInfo.getLocation(agentInfo.getID());
        return Util.findDistanceTo(target, location.first(), location.second()) < maxClearDistance;
    }

    public Pair<Blockade, Integer> findNearestBlockade(Set<Blockade> blockades) {

        if (blockades == null || blockades.isEmpty()) {
            return null;
        }

        Pair<Integer, Integer> location = worldInfo.getLocation(agentInfo.getID());
        int x = location.first();
        int y = location.second();


        int minDistance = Integer.MAX_VALUE;
        Pair<Blockade, Integer> nearestBlockadePair = new Pair<>(blockades.iterator().next(), 0);
        int tempDistance;
        for (Blockade blockade : blockades) {
            tempDistance = Util.findDistanceTo(blockade, x, y);
            if (tempDistance < minDistance) {
                minDistance = tempDistance;
                nearestBlockadePair = new Pair<>(blockade, minDistance);
            }
        }

        return nearestBlockadePair;
    }

    public Set<RangelBlockade> getRoadObstacles(RangelRoad RangelRoad, Pair<Integer, Integer> firstPoint, Pair<Integer, Integer> secondPoint) {
        Set<RangelBlockade> obstacles = new HashSet<>();
        for (RangelBlockade blockade : RangelRoad.getRangelBlockades()) {
            Point2D sourcePoint = Util.getPoint(firstPoint);
            Point2D endPoint = Util.getPoint(secondPoint);
            if (blockade.getPolygon().contains(firstPoint.first(), firstPoint.second()) || Util.intersections(blockade.getPolygon(), new Line2D(sourcePoint, endPoint)).size() > 0
                    || Util.findDistanceTo(blockade.getParent(), firstPoint.first(), firstPoint.second()) < 500
                    || Util.findDistanceTo(blockade.getParent(), secondPoint.first(), secondPoint.second()) < 500) {
                obstacles.add(blockade);
            }
        }

        return obstacles;
    }


    public Set<Blockade> getBlockadesInRange(Pair<Integer, Integer> targetLocation, Set<Blockade> blockadeSeen, int range) {
        Set<Blockade> blockades = new HashSet<>();
        for (Blockade blockade : blockadeSeen) {
            if (Util.findDistanceTo(blockade, targetLocation.first(), targetLocation.second()) < range) {
                blockades.add(blockade);
            }
        }
        return blockades;
    }

    public Set<Blockade> getTargetRoadBlockades(Road road) {
        Set<Blockade> blockades = new FastSet<>();
        if (worldInfo.getChanged().getChangedEntities().contains(road.getID()) && road.isBlockadesDefined()) {
            for (EntityID entityID : road.getBlockades()) {
                blockades.add((Blockade) worldInfo.getEntity(entityID));
            }
        }
        return blockades;
    }

    public Pair<Line2D, Line2D> getClearSecureLines(Line2D clearLine1, Line2D clearLine2, double clearRadius, double distance) {
        Line2D l1, l2;
        double x1, y1, x2, y2;
        Point2D origin1, endPoint1;
        Point2D origin2, endPoint2;
        double ratio = distance / (clearRadius * 2);
        double minX1, maxX1, minY1, maxY1;
        double minX2, maxX2, minY2, maxY2;

        origin1 = clearLine1.getOrigin();
        endPoint1 = clearLine1.getEndPoint();
        origin2 = clearLine2.getOrigin();
        endPoint2 = clearLine2.getEndPoint();

        minX1 = Math.min(origin1.getX(), origin2.getX());
        maxX1 = Math.max(origin1.getX(), origin2.getX());
        minY1 = Math.min(origin1.getY(), origin2.getY());
        maxY1 = Math.max(origin1.getY(), origin2.getY());
        minX2 = Math.min(endPoint1.getX(), endPoint2.getX());
        maxX2 = Math.max(endPoint1.getX(), endPoint2.getX());
        minY2 = Math.min(endPoint1.getY(), endPoint2.getY());
        maxY2 = Math.max(endPoint1.getY(), endPoint2.getY());

        x1 = ratio * (maxX1 - minX1);
        y1 = ratio * (maxY1 - minY1);
        x2 = ratio * (maxX2 - minX2);
        y2 = ratio * (maxY2 - minY2);

        Point2D p1 = new Point2D(
                origin1.getX() == maxX1 ? maxX1 - x1 : minX1 + x1,
                origin1.getY() == maxY1 ? maxY1 - y1 : minY1 + y1);
        Point2D p2 = new Point2D(
                endPoint1.getX() == maxX2 ? maxX2 - x2 : minX2 + x2,
                endPoint1.getY() == maxY2 ? maxY2 - y2 : minY2 + y2);
        l1 = new Line2D(p1, p2);


        p1 = new Point2D(
                origin2.getX() == maxX1 ? maxX1 - x1 : minX1 + x1,
                origin2.getY() == maxY1 ? maxY1 - y1 : minY1 + y1);
        p2 = new Point2D(
                endPoint2.getX() == maxX2 ? maxX2 - x2 : minX2 + x2,
                endPoint2.getY() == maxY2 ? maxY2 - y2 : minY2 + y2);
        l2 = new Line2D(p1, p2);

        l1 = Util.clipLine(l1, Util.lineLength(l1) - (scenarioInfo.getClearRepairRad() >> 1) + 100, true);
        l2 = Util.clipLine(l2, Util.lineLength(l2) - (scenarioInfo.getClearRepairRad() >> 1) + 100, true);
        return new Pair<>(l1, l2);
    }


    private RangelRoad getRangelRoad(EntityID roadId) {
        return worldHelper.getRangelRoad(roadId);
    }
}
